#!/usr/bin/env python3

import rospy
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
import struct
import serial
import numpy as np
from drone_pkg.msg import SendString


#发送速度协议
def ros_send_serial(x,y,z,yaw,flag):#flag=0为目标速度，flag=1为真实速度
  global ser
  sumcheck=0
  addcheck=0
  uart_buf=np.zeros(14,dtype=np.uint8)
  uart_buf[0]=0xaa
  
  uart_buf[1]=0xff
  if flag==0:
    uart_buf[2]=0x0b
  elif flag==1:
    uart_buf[2]=0x07
  else:
    return
  
  uart_buf[3]=8
  uart_buf[4]=(x&(0xFF00))>>8
  uart_buf[5]=x&0xFF
  uart_buf[6]=(y&(0xFF00))>>8
  uart_buf[7]=y&0xFF
  uart_buf[8]=(z&(0xFF00))>>8
  uart_buf[9]=z&0xFF
  uart_buf[10]=(yaw&(0xFF00))>>8
  uart_buf[11]=yaw&0xFF
  for i in range(12):
  
    sumcheck+=uart_buf[i]
    addcheck+=sumcheck
  uart_buf[12]=sumcheck
  uart_buf[13]=addcheck
  buf_end=struct.pack('!'+'B'*len(uart_buf),*uart_buf)
  rospy.loginfo(uart_buf)
  ser.write(buf_end)


#控制速度发送函数z是高度不是z轴速度
def callback(vel):
  x = vel.linear.x*100
  y = vel.linear.y*100
  yaw = vel.angular.z*180/3.1415926
  z = vel.linear.z
  rospy.loginfo("%.3f, %.3f, %.3f, %.3f", x, y, yaw, z)
  ros_send_serial(int (x), int (y), int(z), int (yaw),0)
  return


#实际速度发送函数
def Vel_callback(odom):
  x = odom.twist.twist.linear.x*100
  y = odom.twist.twist.linear.y*100
  yaw = odom.twist.twist.angular.z*180/3.1415826
  rospy.loginfo("%.3f, %.3f, %.3f", x, y, yaw)
  ros_send_serial(int (x), int (y), 0, int (yaw), 1)
  return

res_string = ''
def str_callback(Str):
  global res_string
  res_string = Str.Str 
  

def vel_subscriber():
  global ser
  

  rospy.init_node('Serial_Send', anonymous=True)
  rospy.Subscriber('/cmd_vel', Twist, callback)
  rospy.Subscriber("/odom_out",Odometry,Vel_callback,queue_size=10)
  rospy.Subscriber('/send_string', SendString, )
  rospy.spin()


if __name__ == '__main__':
    ser = serial.Serial('/dev/ttyS6', 500000)
    vel_subscriber()
    
    
    
    
    
